#include "center_detection/center_detector.hpp"
#include "center_detection/center_json.hpp"
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <std_msgs/msg/string.hpp>
#include <mutex>

namespace center_detection{

    CenterDetector::CenterDetector() : Node("source_center_detector") {
        
        //声明参数
        declare_parameters();

        //初始化处理组件
        init_components();

        // 设置默认检测器模式
        detection_mode_ = "barrel";

        //创建订阅者和发布者
        setup_ros_interface();

        RCLCPP_INFO(this->get_logger(), "source center detector is now working!!!");
    }

    void CenterDetector::declare_parameters(){

        //放射源桶中心检测参数
        this->declare_parameter("barrel.search_start_z", 0.6);
        this->declare_parameter("barrel.search_end_z", 0.4);
        this->declare_parameter("barrel.search_step", 0.01);
        this->declare_parameter("barrel.min_points_in_slice", 30);        
        this->declare_parameter("barrel.barrel_height", 0.2);
        this->declare_parameter("barrel.barrel_radius", 0.2);
        this->declare_parameter("barrel.fixed_point_a_x", 3.8);
        this->declare_parameter("barrel.fixed_point_a_y", 3.8);

        //井盖中心检测参数
        this->declare_parameter("cover.handle_search_start_z", 0.6);
        this->declare_parameter("cover.handle_search_end_z", 0.2);
        this->declare_parameter("cover.handle_search_step", 0.01);
        this->declare_parameter("cover.handle_height_range", 0.13);
        this->declare_parameter("cover.handle_min_points", 200);
        this->declare_parameter("cover.handle_z_offset", 0.13);
        this->declare_parameter("cover.circle_search_start_z", 0.33);
        this->declare_parameter("cover.circle_search_end_z", 0.2);
        this->declare_parameter("cover.circle_search_step", 0.01);
        this->declare_parameter("cover.circle_height_range", 0.1);
        this->declare_parameter("cover.circle_min_points", 50);
        this->declare_parameter("cover.circle_min_points_final", 30);
        this->declare_parameter("cover.quantization_step", 0.01);
        this->declare_parameter("cover.percentile_low", 0.05);
        this->declare_parameter("cover.percentile_high", 0.95);
        this->declare_parameter("cover.center_merge_threshold", 0.03);

        //锥形放射源检测参数
        this->declare_parameter("neutron.search_start_z", 0.35);
        this->declare_parameter("neutron.search_end_z", 0.2);
        this->declare_parameter("neutron.search_step", 0.01);
        this->declare_parameter("neutron.min_points_in_slice", 50);
        this->declare_parameter("neutron.neutron_height", 0.2);
        this->declare_parameter("neutron.top_width", 0.2);
        this->declare_parameter("neutron.top_height", 0.2);
        this->declare_parameter("neutron.width_difference_threshold", 0.2);
        this->declare_parameter("neutron.height_difference_threshold", 0.2);

    }

    
    void CenterDetector::init_components(){

        //初始化监测管理器
        detection_manager_ = std::make_unique<DetectionManager>();

        //创建桶检测器
        auto barrel_detector = std::make_shared<BarrelDetector>();
        detection_manager_->registerDetector("barrel", barrel_detector);

        // 创建井盖检测器
        auto cover_detector = std::make_shared<CoverDetector>();
        detection_manager_->registerDetector("cover", cover_detector);

        // 创建锥形放射源检测器
        auto neutron_detector = std::make_shared<NeutronDetector>();
        detection_manager_->registerDetector("neutron", neutron_detector);


    }

    void CenterDetector::setup_ros_interface(){
        
        // 创建gpu处理后的点云消息的订阅者
        cloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "mqtt_reconstructed_cloud", 10, std::bind(&CenterDetector::cloud_callback, this, std::placeholders::_1));

        // 创建检测模式话题订阅者
        detection_mode_sub_ = this->create_subscription<std_msgs::msg::String>(
            "detection_mode_fsm", 10, std::bind(&CenterDetector::detection_mode_callback, this, std::placeholders::_1));

        // 创建发布者

        // 发布可视化中心点标记
        pub_barrel_center_ = this->create_publisher<geometry_msgs::msg::PointStamped>("/barrel_center", 10);
        pub_cover_center_ = this->create_publisher<geometry_msgs::msg::PointStamped>("/cover_center", 10);
        pub_neutron_center_ = this->create_publisher<geometry_msgs::msg::PointStamped>("/neutron_center", 10);

        // 发布中心点坐标json数据
        pub_center_json_ = this->create_publisher<std_msgs::msg::String>("/center_json", 10);
        

        //设置检测器参数
        auto barrel_detector = std::dynamic_pointer_cast<BarrelDetector>(
            detection_manager_->getDetector("barrel"));
        if (barrel_detector) {
            barrel_detector->setParameters(this);
        }
        
        auto cover_detector = std::dynamic_pointer_cast<CoverDetector>(
            detection_manager_->getDetector("cover"));
        if (cover_detector) {
            cover_detector->setParameters(this);
        }

        auto neutron_detector = std::dynamic_pointer_cast<NeutronDetector>(
            detection_manager_->getDetector("neutron"));
        if (neutron_detector) {
            neutron_detector->setParameters(this);
        }

    }


    void CenterDetector::detection_mode_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        std::lock_guard<std::mutex> lock(detection_mode_mutex_);
        detection_mode_ = msg->data;
        RCLCPP_INFO(this->get_logger(), "Received detection mode from state machine: %s", 
                   detection_mode_.c_str()); 
    }

    void CenterDetector::cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {

        // 转换为PCL点云
        auto laser_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
        pcl::fromROSMsg(*msg, *laser_cloud);
        if(laser_cloud->empty()) {
            return;
        }

        // 确定对应放射源检测器进行中心点识别
        perform_detection(laser_cloud);

    }
    

    void CenterDetector::perform_detection(const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud) {

        // 使用从状态机获取的检测模式
        // 使用互斥锁锁定当前检测模式线程
        std::string current_mode;
        {
            std::lock_guard<std::mutex> lock(detection_mode_mutex_);
            current_mode = detection_mode_;
        }


        //仅使用桶检测
        if(current_mode == "barrel") {
            geometry_msgs::msg::PointStamped barrel_center;
            if(detection_manager_->detect("barrel", cloud, barrel_center)) {
                // 发布中心点标记
                barrel_center.header.stamp = this->now();
                barrel_center.header.frame_id = "map";
                pub_barrel_center_->publish(barrel_center);

                // 发布中心点坐标json
                json_coord_.set_from_coord(barrel_center.point.x, barrel_center.point.y, barrel_center.point.z);
                json_msg_.data = json_coord_.get_json_string();
                pub_center_json_->publish(json_msg_);

            }
        }

        //仅使用井盖检测
        else if(current_mode == "cover") {
            geometry_msgs::msg::PointStamped cover_center;
            if(detection_manager_->detect("cover", cloud, cover_center)) {
                // 发布中心点标记
                cover_center.header.stamp = this->now();
                cover_center.header.frame_id = "map";
                pub_cover_center_->publish(cover_center);
                
                // 发布中心点坐标json
                json_coord_.set_from_coord(cover_center.point.x, cover_center.point.y, cover_center.point.z);
                json_msg_.data = json_coord_.get_json_string();
                pub_center_json_->publish(json_msg_);

            }
        }

        //仅使用锥形放射源检测
        else if(current_mode == "neutron") {
            geometry_msgs::msg::PointStamped neutron_center;
            if(detection_manager_->detect("neutron", cloud, neutron_center)) {
                // 发布中心点标记
                neutron_center.header.stamp = this->now();
                neutron_center.header.frame_id = "map";
                pub_neutron_center_->publish(neutron_center);
                
                // 发布中心点坐标json
                json_coord_.set_from_coord(neutron_center.point.x, neutron_center.point.y, neutron_center.point.z);
                json_msg_.data = json_coord_.get_json_string();
                pub_center_json_->publish(json_msg_);
                
            }
        }
    }
}


int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<center_detection::CenterDetector>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0 ;
}